
#include "fc_sensors.h"
#include "filters.h"
#include "zino.h"
#include "spl06_01.h"
#include <math.h>
#include "icm42688p_read_polling.h"
#include "filters.h"
#include "board.h"
#define DBG_TAG "sensors"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

extern float F1_wave[12];

/**Latesd data for sensor, Max 1000Hz */
fc_sensor_data_t fc_sensor_data = {0};

/**processed data, Max 100Hz */
zdrone_state_t zdrone_state = {0};

// bool baro_online = false;
// bool acc_gyro_online = false;
// bool laser_online = false;
// bool opticalflow_online = false;
// bool acc_hardfault = false;
// bool gyro_hardfault = false;
// static uint8_t acc_hardfault_cnt = 0;
// static uint8_t gyro_hardfault_cnt = 0;
// rt_device_t temp_dev = RT_NULL;
// rt_device_t baro_dev = RT_NULL;
// rt_device_t acce_dev = RT_NULL;
// rt_device_t gyro_dev = RT_NULL;
// rt_device_t tof_dev = RT_NULL;
// rt_device_t optical_dev = RT_NULL;

// struct rt_sensor_data tof_data;
// struct rt_sensor_data optical_data;
// struct rt_sensor_data temp_data;
// struct rt_sensor_data baro_data;
// struct rt_sensor_data acce_data;
// struct rt_sensor_data gyro_data;

// extern void flipinghandle_fun(void);
// void sensordata_update(void *parameter)
// {

// #define TEMP_DEV "temp_spl"
// #define BARO_DEV "baro_spl"
// #define ACCE_DEV "acce_icm"
// #define GYRO_DEV "gyro_icm"
// #define TOF_DEV "tof_NDS0"
// #define OPTICAL_DEV "of_pmw39"
// #define RUNINNG_HZ(X) if (_1ms_cnt % (1000 / (X)) == 0)

//     static uint16_t _1ms_cnt = 0;
//     rt_thread_mdelay(5); // NOTE wait for serial empty

//     if (ZeroSave()->gyro_calibrated && ZeroSave()->acc_calibrated && ZeroSave()->flag == ZERO_SAVED_FLAG)
//     {
//         LOG_D("Loading acc gyro bias:");
//         LOG_D("ACC: %d %d %d", ZeroSave()->accoffset.x, ZeroSave()->accoffset.y, ZeroSave()->accoffset.z);
//         LOG_D("GYRO: %d %d %d", ZeroSave()->gyrooffset.x, ZeroSave()->gyrooffset.y, ZeroSave()->gyrooffset.z);
//         fc_sensor_data.acc.acc_offset.isdatacalibrated = true;
//         fc_sensor_data.gyro.gyro_offset.isdatacalibrated = true;
//     }
//     else
//     {
//         ZeroSave()->acc_cutoff = ACC_LPF_CUTOFF;
//         ZeroSave()->gyro_cutoff = GYRO_LPF_CUTOFF;
//         LOG_D("No acc gyro bias");
//         LOG_D("acc gyro start calibration");
//     }
//     uint8_t motion_cnt = 0;
//     uint8_t motion_freq=0;
//     const rt_base_t MOTION_PIN = 44;//pin num PC.12 -> 44
//     rt_pin_mode(MOTION_PIN, PIN_MODE_INPUT_PULLUP);
//     statecontrol_init();
//     fc_motor_on();
//     while (1)
//     {
//         uint32_t now_time = rt_tick_get();
//         RUNINNG_HZ(100)
//         {
//             if (!baro_online)
//             {
//                 FC_ERROR_SET(FC_BARO_HARDFAULT);
//                 baro_dev = rt_device_find(BARO_DEV);
//                 temp_dev = rt_device_find(TEMP_DEV);
//                 if (temp_dev != RT_NULL && baro_dev != RT_NULL)
//                 {
//                     rt_device_open(baro_dev, RT_DEVICE_FLAG_RDWR);
//                     rt_device_open(temp_dev, RT_DEVICE_FLAG_RDWR);
//                     baro_online = true;
//                     FC_ERROR_RESET(FC_BARO_HARDFAULT);
//                     LOG_D("baro ok\n");
//                 }
//             }
//             else
//             {
//                 rt_device_read(baro_dev, 0, &baro_data, 1);
//                 rt_device_read(temp_dev, 0, &temp_data, 1);
//                 sensor_baro_handle(&baro_data, &temp_data);
//                 // rt_kprintf("baro_data:%d\n",baro_data.data.baro);
//             }
//             static uint32_t last_time1 = 0;
//             float dt = (float)(now_time - last_time1) * 0.001f;
//             last_time1 = now_time;
//             positionEstimate(&fc_sensor_data, &fc_ctrl_imu, &zdrone_state, dt);

//             position_control(&fc_control, &setPoint, &fc_sensor_data, &zdrone_state, dt);

//             /*NOTE SETPION
//             FORWARD: +Pitch, BACKWARD: -Pitch
//             LEFT: -Roll, RIGHT: +Roll
//              */

//             // rt_printf_float("sYaw: ",setPoint.Angle.yaw," \n");

//             // rt_printf_float("Vx: ",zdrone_state.vel.x," ");
//             // rt_printf_float("Vy: ",zdrone_state.vel.y," ");
//             // rt_printf_float("VxO: ",pidData[PID_POSITION_VX].out," ");
//             // rt_printf_float("VyO: ",pidData[PID_POSITION_VY].out," ");
//             // rt_printf_float("sPitch: ",setPoint.Angle.pitch," ");
//             // rt_printf_float("sRoll: ",setPoint.Angle.roll," \n");
//             // F1_wave[0] = zdrone_state.vel.x;
//             // F1_wave[1] = zdrone_state.vel.y;
//             // F1_wave[2] = pidData[PID_POSITION_VX].out;
//             // F1_wave[3] = pidData[PID_POSITION_VY].out;
//             // F1_wave[4] = setPoint.Angle.pitch;
//             // F1_wave[5] = setPoint.Angle.roll;
//         }

//         RUNINNG_HZ(1000)
//         {
//             static uint32_t last_time2 = 0;
//             float dt = (float)(now_time - last_time2) * 0.001f;
//             last_time2 = now_time;
//             if (!acc_gyro_online)
//             {
//                 FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
//                 acce_dev = rt_device_find(ACCE_DEV);
//                 gyro_dev = rt_device_find(GYRO_DEV);
//                 if (acce_dev != RT_NULL && gyro_dev != RT_NULL)
//                 {
//                     rt_device_open(acce_dev, RT_DEVICE_FLAG_RDWR);
//                     rt_device_open(gyro_dev, RT_DEVICE_FLAG_RDWR);
//                     acc_gyro_online = true;
//                     FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
//                     LOG_D("acc_gyro ok\n");
//                 }
//             }
//             else
//             {
//                 acce_data.type = RT_SENSOR_CLASS_NONE;
//                 gyro_data.type = RT_SENSOR_CLASS_NONE;
//                 int ret_acc = rt_device_read(acce_dev, 0, &acce_data, 1);
//                 int ret_gyro = rt_device_read(gyro_dev, 0, &gyro_data, 1);

//                 sensor_acc_process(&acce_data);
//                 sensor_gyro_process(&gyro_data);

//                 if ((acce_data.type != RT_SENSOR_CLASS_ACCE) || (ret_acc != 1))
//                 {
//                     if (acc_hardfault_cnt < 100)
//                     {
//                         acc_hardfault_cnt++;
//                     }
//                     else
//                     {
//                         acc_hardfault = true;
//                     }
//                 }
//                 else
//                 {
//                     acc_hardfault_cnt = 0;
//                     acc_hardfault = false;
//                 }
//                 if ((gyro_data.type != RT_SENSOR_CLASS_GYRO) || (ret_gyro != 1))
//                 {
//                     if (gyro_hardfault_cnt < 100)
//                     {
//                         gyro_hardfault_cnt++;
//                     }
//                     else
//                     {
//                         gyro_hardfault = true;
//                     }
//                 }
//                 else
//                 {
//                     gyro_hardfault_cnt = 0;
//                     gyro_hardfault = false;
//                 }

//                 if (acc_hardfault || gyro_hardfault)
//                 {
//                     FC_ERROR_SET(FC_ACC_GYRO_HARDFAULT);
//                     if (FC_ERROR_CHECK(FC_ARMED))
//                     {
//                         FC_ERROR_RESET(FC_ARMED);
//                     }
//                 }
//                 else
//                 {
//                     FC_ERROR_RESET(FC_ACC_GYRO_HARDFAULT);
//                 }

//                 MahonyAHRS(&fc_ctrl_imu, &fc_sensor_data, dt);

//                 static uint16_t overContrl = 0;
//                 if (fc_control.MixerRange > 900)
//                 {
//                     // LOG_D("MixerRange:%d",fc_control.MixerRange);
//                     if (overContrl < 100)
//                     {
//                         overContrl++;
//                     }
//                     if (overContrl >= 100)
//                     {
//                         overContrl = 0;
//                     }
//                 }
//                 else
//                 {
//                     overContrl = 0;
//                 }
//             }
//             if (get_fc_main_state() != true)
//             {
//                 /*翻转功能*/
//                 flipinghandle_fun();
//                 /*PID控制*/
//                 statecontrol(&fc_control, &setPoint, &fc_sensor_data, &zdrone_state, dt);
//                 /**电机控制 */
//                 fc_motor_set_output(&fc_control);
//             }
//         }

//         RUNINNG_HZ(30)
//         {
//             if (!laser_online)
//             {
//                 FC_ERROR_SET(FC_LASER_HARDFAULT);
//                 tof_dev = rt_device_find(TOF_DEV);
//                 if (tof_dev != RT_NULL)
//                 {
//                     rt_device_open(tof_dev, RT_DEVICE_FLAG_RDWR);
//                     laser_online = true;
//                     FC_ERROR_RESET(FC_LASER_HARDFAULT);
//                     // LOG_D("TOF ok\n");
//                 }
//                 rt_memset(&tof_data, 0, sizeof(tof_data));
//             }
//             else
//             {
//                 if (rt_device_read(tof_dev, 0, &tof_data, 1) == 1)
//                 {
//                     sensor_tof_handle(&tof_data);
//                 }
//                 else
//                 {
//                     rt_memset(&tof_data, 0, sizeof(tof_data));
//                 }
//             }
//         }
//         RUNINNG_HZ(1000)
//         {
//             if (!opticalflow_online)
//             {
//                 FC_ERROR_SET(FC_OPTICALFLOW_HARDFAULT);
//                 optical_dev = rt_device_find(OPTICAL_DEV);
//                 if (optical_dev != RT_NULL)
//                 {
//                     rt_device_open(optical_dev, RT_DEVICE_FLAG_RDWR);
//                     opticalflow_online = true;
//                     FC_ERROR_RESET(FC_OPTICALFLOW_HARDFAULT);
//                     // LOG_D("optical ok\n");
//                 }
//             }
//             else
//             {
//                 if(rt_pin_read(MOTION_PIN) == PIN_LOW)
//                 {
//                     if (rt_device_read(optical_dev, 0, &optical_data, 1) == 1)
//                     {
//                         sensor_opticlFlow_handle(&optical_data);
//                         motion_cnt ++;
//                     }
//                     else
//                     {
//                         rt_memset(&optical_data, 0, sizeof(optical_data));
//                     }
//                 }
//             }
//         }

//         _1ms_cnt++;

//         if (_1ms_cnt >= 1000)
//         {
//             _1ms_cnt = 0;
//             motion_freq = motion_cnt;
//             motion_cnt = 0;
//             // rt_kprintf("motion_freq:%d\n",motion_freq);
//         }

//         rt_thread_mdelay(1);
//     }
// }
// /**
//  * @description: 读取原始数据，把原始数据读到sensordata中
//  * @return {*}
//  */
// int sensor_task_entry(void)
// {
//     rt_thread_t sensor_data_thread = rt_thread_create("sensors", sensordata_update, RT_NULL, 1024, 1, 1);
//     if (sensor_data_thread != RT_NULL)
//     {
//         rt_thread_startup(sensor_data_thread);
//     }
//     else
//     {
//         LOG_E("sensor_data_thread create failed.");
//     }
//     return 0;
// }
// ZINO_APP_EXPORT(sensor_task_entry);
// MSH_CMD_EXPORT(sensordata_update, sensor task entry);
